clear all; close all;clc;
cc=0;
    Norder = 3;
% freqs= [ 300 600 1500 3000]; %From published paper
freqs = [200 500 1000 2000]
nturns = [0.95 2.2 3.4 4.1];
x_loc = [0:-5:-175 180:-5:5];
% y_loc = [-57,-30,-15,0,15,30,45,60,75];

fs = 32.1e3;
dur=5;
Nump='ABCD';
soundwave=randn(round(fs*dur),1); % BB
f1=figure('position',[100 100 1200 400]);
out_hart = ncread('Acoustic/Subject1/Subject1_HRIRs.sofa',"Data.IR");
% out_hart = ncread('Acoustic/Subject2/Subject2_HRIRs.sofa',"Data.IR");
% x_chosen = [18:-1:1 71:-1:56];
x_chosen = [19:-2:1 71:-2:54];
x_loc(x_chosen)
Elevation = 4; % ear level, 4
for ifreql = 1:length(freqs)
    freq = freqs(ifreql)
    for i=1:length(x_chosen)
        Index = (Elevation - 1)*72+x_chosen(i);%
        h_l = squeeze(out_hart(:,1,Index)); % Left HRIRs
        h_r = squeeze(out_hart(:,2,Index)); % Right HRIRs
        x1 = conv(soundwave,h_l,'same');
        x2 = conv(soundwave,h_r,'same');
        % % % %      short time fourier transform:
        [SL,SR,freq_pos, flag01]=mySTFT(x1,x2,fs);
        %      normalization:
        [SL1,SR1]=Normalize(SL,SR);
        rad = 0.2;
        [Y Ind] = min(abs(freq_pos - freq));
        X_P = [real(SR1(Ind,flag01>0))' imag(SR1(Ind,flag01>0))'];
        figure(f1)
        subplot(1,4,ifreql)
        plot(X_P(:,1),X_P(:,2),'.','Markersize',4);
        [IDX, C] = kmeans(X_P, 1,'Distance','cityblock','Replicates',5);
        F_centers(i,:) = C;
        set(gca,'Xtick',-1:0.5:1)
        set(gca,'Ytick',-1:0.5:1)
        title([Nump(ifreql) '. ' num2str(freq), ' Hz'],'fontsize',14);
        %     axis([-1 1 -1 1])
        hold on;
        plot(-1:1,[0,0,0],'k', 'LineWidth', 0.5);
        plot([0,0,0],-1:1,'k','LineWidth',0.5)
        axis square
        box off
        txy = C;
        flag_plot = 1;
        % if ifreql<3 & mod(i,2)==0
        %     flag_plot = 0;
        % end
        % if flag_plot==1
            s=text(txy(1)-0.02,txy(2)-0.12,[num2str(x_loc(x_chosen(i))) '^{\circ}'],'horizontalalignment','center');
        % end
        %     s=text(txy(1),txy(2),[num2str(G(i)) '^{\circ}'],'horizontalalignment','center')
        set(s,'fontsize',10)
        grid on
        set(gca,'Xtick',-1:0.5:1)
        set(gca,'Ytick',-1:0.5:1,'fontsize',10)
        axis([-1.1 1.1 -1.1 1.1])
        xlabel('Real','fontsize',12); ylabel('Imaginary','fontsize',12);

        pause(0.01)
    end

    for jk = 1:length(F_centers)
        r_nonlinear(jk) = sqrt( F_centers(jk,1)^2 +  F_centers(jk,2)^2 );
    end

    xx = 1:length(F_centers);
    P = polyfit(xx,r_nonlinear,Norder)
    xx_fine=1:0.01:length(F_centers);
    fitted_r=0;
    % fitted_r = P(1).*xx_fine.^2 + P(2).*xx_fine + P(3);
    for i=1:length(P)
        fitted_r=fitted_r+P(i).*xx_fine.^(Norder+1-i);
    end
    % s=plot(F_centers(:,1),F_centers(:,2),'ok');set(s,'markersize',13,'linewidth',1.5)
    nturn=nturns(ifreql);
    %==============================
    % [x_spiral y_spiral]=f_plot_spiral([0 0], [F_centers(1,:); F_centers(end,:)],nturn,fitted_r);
    out=f_get_spiral(F_centers,Norder,nturns(ifreql));
    x_spiral=out.x_spiral;
    y_spiral=out.y_spiral;
    % figure(f1)
    % subplot(2,3,FNum(coungf))
    plot(x_spiral,y_spiral,'color',[0.5 0.5 0.5]) ; % nturns crossings, including end point
    % hold on
    %     exportgraphics(gcf, 'CC_Figure_5_Various_Freq.png', 'Resolution', 300,'ContentType', 'vector')

    % eval(['save Freq_centers' num2str(freq) ' F_centers x_spiral y_spiral'])
end

exportgraphics(gcf, 'BB_Figure2_Various_Freq.png', 'Resolution', 600,'ContentType', 'vector')

